#coding=utf-8

import xlwt
import os
import yaml
from datetime import datetime
class DataRecorder:
    def __init__(self):
        '''
            初始化
        '''
        '''
            基础配置信息
        '''
        self.columnsRot = ['时间', 'Base-Roll', 'Base-Pitch', 'Base-Yaw', '左脚-Roll', '左脚-Pitch', '左脚-Yaw',
                            '右脚-Roll', '右脚-Pitch', '右脚-Yaw']
        self.columnsAccel= ['时间', 'Base-X', 'Base-Y', 'Base-Z', '左脚-X', '左脚-Y', '左脚-Z',
                            '右脚-X', '右脚-Y', '右脚-Z']
        self.columnsGyro = ['时间', 'Base-RX', 'Base-RY', 'Base-RZ', '左脚-RX', '左脚-RY', '左脚-RZ',
                             '右脚-RX', '右脚-RY', '右脚-RZ']
        self.columnsPressure = ['时间', '左脚-FL', '左脚-FR', '左脚-BL','左脚-BR',
                             '右脚-FL', '右脚-FR', '右脚-BL','右脚-BR']
        self.columnsZMP = ['时间', '左脚-X', '左脚-Y',
                                '右脚-X', '右脚-Y']
        self.columnsJointsRaw = ['时间', 'JL1(踝)','JL2','JL3','JL4','JL5','JL6(胯)','JR1(踝)','JR2','JR3','JR4','JR5','JR6(胯)']
        fileName=datetime.now().strftime('%Y-%m-%d %H-%M-%S')

        with open('config/config.yaml',encoding='utf-8') as file:
            config=yaml.load(file,Loader=yaml.FullLoader)

        self.fileName=os.path.join(config['data_storage'],fileName)+'.xls'
        self.workbook = xlwt.Workbook()
        self.sRot = self.workbook.add_sheet('姿态数据')
        self.sAccel = self.workbook.add_sheet('加速度原始')
        self.sGyro = self.workbook.add_sheet('陀螺仪原始')
        self.sPress = self.workbook.add_sheet('脚底压力')
        self.sZMP = self.workbook.add_sheet('压力重心')
        self.sJRaw = self.workbook.add_sheet('原始关节')
        self.sJMod = self.workbook.add_sheet('修正后关节')
        self.WriteHeader()

    def WriteHeader(self):
        #   姿态
        for col_index, col_name in enumerate(self.columnsRot):
            self.sRot.write(0, col_index, col_name)
        #   Accelometer
        for col_index, col_name in enumerate(self.columnsAccel):
            self.sAccel.write(0, col_index, col_name)
        #   Gyroson
        for col_index, col_name in enumerate(self.columnsGyro):
            self.sGyro.write(0, col_index, col_name)
        #   pressure
        for col_index, col_name in enumerate(self.columnsPressure):
            self.sPress.write(0, col_index, col_name)
        #   ZMP
        for col_index, col_name in enumerate(self.columnsZMP):
            self.sZMP.write(0, col_index, col_name)

        for col_index, col_name in enumerate(self.columnsJointsRaw):
            #   原始关节数值
            self.sJRaw.write(0, col_index, col_name)
            #   修正关节数值
            self.sJMod.write(0, col_index, col_name)

    def FeedRot(self,t,vecB,vecL,vecR):
        data=[t,vecB[0],vecB[1],vecB[2],vecL[0],vecL[1],vecL[2],vecR[0],vecR[1],vecR[2]]
        row_index = self.sRot.last_used_row + 1
        for col_index, value in enumerate(data):
            self.sRot.write(row_index, col_index, value)
    def FeedAccel(self,t, vecB,vecL,vecR):
        data=[t,vecB[0],vecB[1],vecB[2],vecL[0],vecL[1],vecL[2],vecR[0],vecR[1],vecR[2]]
        row_index = self.sAccel.last_used_row + 1
        for col_index, value in enumerate(data):
            self.sAccel.write(row_index, col_index, value)
    def FeedGyro(self, t,vecB,vecL,vecR):
        data=[t,vecB[0],vecB[1],vecB[2],vecL[0],vecL[1],vecL[2],vecR[0],vecR[1],vecR[2]]
        row_index = self.sGyro.last_used_row + 1
        for col_index, value in enumerate(data):
            self.sGyro.write(row_index, col_index, value)
    def FeedPressure(self,t, vecL,vecR):
        data=[t,vecL[0],vecL[1],vecL[2],vecL[3],vecR[0],vecR[1],vecR[2],vecR[3]]
        row_index = self.sPress.last_used_row + 1
        for col_index, value in enumerate(data):
            self.sPress.write(row_index, col_index, value)
    def FeedZMP(self,t, vecL,vecR):
        data=[t,vecL[0],vecL[1],vecR[0],vecR[1]]
        row_index = self.sZMP.last_used_row + 1
        for col_index, value in enumerate(data):
            self.sZMP.write(row_index, col_index, value)
    def FeedJRaw(self,t,datal,datar):
        row_index = self.sZMP.last_used_row + 1
        dat2Write=[t]
        dat2Write.extend(list(datal))
        dat2Write.extend(list(datar))
        for col_index, value in enumerate(dat2Write):
            self.sJRaw.write(row_index, col_index, value)

    def FeedJMod(self, t, datal, datar):
        row_index = self.sZMP.last_used_row + 1
        dat2Write = [t]
        dat2Write.extend(list(datal))
        dat2Write.extend(list(datar))
        for col_index, value in enumerate(dat2Write):
            self.sJMod.write(row_index, col_index, value)
    def Save(self, fileName=''):
        # 保存数据到文件
        if fileName=='':
            self.workbook.save(self.fileName)
        else:
            self.workbook.save(fileName)
